#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include <ncurses.h>
#include <signal.h>

// 发布者声明
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher;

// 信号处理函数
void signal_handler(int signum) {
    // 处理Ctrl+C（SIGINT）信号
    if (signum == SIGINT) {
        rclcpp::shutdown();  // 关闭ROS 2节点
        endwin();  // 结束ncurses
        exit(0);   // 退出程序
    }
}

// 输入处理函数
void process_input(int ch) {
    geometry_msgs::msg::Twist msg;
    // 基于输入字符控制turtle1
    if (ch == 'w') {
        msg.linear.x = 3.0;
    } else if (ch == 's') {
        msg.linear.x = -3.0;
    } else if (ch == 'a') {
        msg.angular.z = 3.0;
    } else if (ch == 'd') {
        msg.angular.z = -3.0;
    } else if (ch == 'p') {
        msg.linear.x = 0.0;
        msg.angular.z = 0.0;  // 紧急停止
    }
    publisher->publish(msg);  // 发布控制消息
}

int main(int argc, char *argv[]) {
    rclcpp::init(argc, argv);

    // 创建ROS 2节点
    rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("keyboard_control_node");

    // 创建cmd_vel话题的发布者
    publisher = node->create_publisher<geometry_msgs::msg::Twist>("/turtle1/cmd_vel", 10);

    // 注册信号处理函数
    signal(SIGINT, signal_handler);  // 捕捉Ctrl+C

    // 初始化ncurses
    initscr();
    cbreak();
    noecho();
    timeout(100); // 100ms等待超时

    printw("Control the turtle with WASD keys.\nPress 'p' for emergency stop.\n");

    geometry_msgs::msg::Twist msg;
    int ch;
    while (rclcpp::ok()) {  // 在ROS 2节点运行时保持循环
        ch = getch();  // 获取一个字符输入

        if (ch == ERR) {
            // 没有输入，发布停止命令
            // msg.linear.x = 0.0;
            // msg.angular.z = 0.0;
            // publisher->publish(msg);
            continue;
        }

        if (ch == 'q') {
            break; // 按'q'退出程序
        }

        process_input(ch);
    }

    // 结束ncurses
    endwin();

    rclcpp::shutdown();
    return 0;
}
